﻿using System;
using System.Collections;
using System.Collections.Generic;
using Aldebaran.Proxies;
using System.Linq;
using System.Text;
using System.Drawing;
using System.ComponentModel;
using System.IO;
using System.Drawing.Imaging;
using Emgu.CV;
using Emgu.CV.Structure;
using Emgu.CV.Util;


namespace NAO_Image_Processing
{
    public struct NaoCamImageFormat
    {
        public string name;
        public int id;
        public int width;
        public int height;
    }

    public class NAOCamera
    {
        private VideoDeviceProxy _videoDeviceProxy = null;
        string _ip = "";
        public List<NaoCamImageFormat> NaoCamImageFormats = new List<NaoCamImageFormat>();

        public NAOCamera()
        {
            NaoCamImageFormat format0 = new NaoCamImageFormat();
            format0.name = "160 * 120";
            format0.id = 0;
            format0.width = 160;
            format0.height = 120;

            NaoCamImageFormat format1 = new NaoCamImageFormat();
            format1.name = "320 * 240";
            format1.id = 1;
            format1.width = 320;
            format1.height = 240;

            NaoCamImageFormat format2 = new NaoCamImageFormat();
            format2.name = "640 * 480";
            format2.id = 2;
            format2.width = 640;
            format2.height = 480;

            NaoCamImageFormat format3 = new NaoCamImageFormat();
            format3.name = "1280 * 960";
            format3.id = 3;
            format3.width = 1280;
            format3.height = 960;

            NaoCamImageFormats.Add(format0);
            NaoCamImageFormats.Add(format1);
            NaoCamImageFormats.Add(format2);
            NaoCamImageFormats.Add(format3);
        }

        public bool Connect(string ip, NaoCamImageFormat format, int ColorSpace, int Fps)
        {
            _ip = ip;
            try
            {
                if (_videoDeviceProxy != null)
                {
                    Disconnect();
                }
                _videoDeviceProxy = new VideoDeviceProxy(ip, 9559);
                try
                {
                    /* Kamera abmelden, bevor Sie angemeldet wird.
                     * Für den Fall, dass die Kamera beim letzten mal nicht abgemeldet wurde.
                     */
                    _videoDeviceProxy.unsubscribe("Camera");
                }
                catch (Exception ex)
                {
                    Console.Error.WriteLine("NAOCamera.Connect.unsubcribe exception" + ex.Message);
                }
                
                /* Kamera anmelden */
                if (_videoDeviceProxy.subscribe("Camera", format.id, ColorSpace, Fps) != "")
                {
                    return true;
                }
                else
                {
                    return false;
                }
            }
            catch (Exception ex)
            {
                _videoDeviceProxy = null;
                Console.Out.WriteLine("NAOCamera.Connect exception: " + ex);
                return false;
            }
        }

        public bool Disconnect()
        {
            try
            {
                if (_videoDeviceProxy != null)
                {
                    _videoDeviceProxy.unsubscribe("Camera");
                    _videoDeviceProxy = null;
                    if (_videoDeviceProxy == null)
                    {
                        return true;
                    }
                    return false;
                }
                return false;
            }
            catch (Exception e)
            {
                Console.Out.WriteLine("NAOCamera.Disconnect exception: " + e);
                return false;
            }
        }

        public Bitmap GetImage(NaoCamImageFormat imageFormat)
        {
            Bitmap returnBitmap = new Bitmap(imageFormat.width, imageFormat.height, PixelFormat.Format24bppRgb);

            try
            {
                if (_videoDeviceProxy != null)
                {
                    unsafe
                    {
                        Object imageObject = _videoDeviceProxy.getImageRemote("Camera");
                        byte[] imageBytes = (byte[])((ArrayList)imageObject)[6];
                        _videoDeviceProxy.releaseImage("Camera");

                        if (imageBytes != null)
                        {
                            BitmapData bmData = returnBitmap.LockBits(new Rectangle(0, 0, returnBitmap.Width, returnBitmap.Height),
                                                       ImageLockMode.WriteOnly,
                                                       PixelFormat.Format24bppRgb);

                            // Pointer auf den Beginn der Pixel-Daten 
                            byte* p = (byte*)bmData.Scan0;
                            int diff = bmData.Stride - returnBitmap.Width * 3;

                            for (int i = 0; i < imageBytes.Length; i++)
                            {
                                *p++ = imageBytes[i];
                                if (i % (returnBitmap.Width * 3) == 0 && i > 0)
                                {
                                    p += diff;
                                }
                            }
                            // Speicher wieder freigeben
                            returnBitmap.UnlockBits(bmData);
                            bmData = null;
                            p = null;
                        }
                    }
                }
            }
            catch (Exception e)
            {
                Console.Out.WriteLine("NAOCamera.GetImage exception: " + e.Message);
            }
            return returnBitmap;
        }

        public bool visionIsAvailable()
        {
            if (_videoDeviceProxy != null)
            {
                return true;
            }
            else
            {
                return false;
            }
        }
    }
}